%initialze model
load('Results\RoughGround\optimizedGains.mat')
BodyMechParams
ControlParams

[groundX, groundZ, groundTheta] = generateGround('flat');

model = 'NeuromuscularModel';
load_system(model)
rtp = Simulink.BlockDiagram.buildRapidAcceleratorTarget(model);

%set gains
rtp = Simulink.BlockDiagram.modifyTunableParameters(rtp, ...
            'GainGAS',               Gains( 1), ...
            'GainGLU',               Gains( 2), ...
            'GainHAM',               Gains( 3), ...
            'GainKneeOverExt',       Gains( 4), ...
            'GainSOL',               Gains( 5), ...
            'GainSOLTA',             Gains( 6), ...
            'GainTA',                Gains( 7), ...
            'GainVAS',               Gains( 8), ...
            'Kglu',                  Gains( 9), ...
            'PosGainGG',             Gains(10), ...
            'SpeedGainGG',           Gains(11), ...
            'hipDGain',              Gains(12), ...
            'hipPGain',              Gains(13), ...
            'kneeExtendGain',        Gains(14), ...
            'kneeFlexGain',          Gains(15), ...
            'kneeHoldGain1',         Gains(16), ...
            'kneeHoldGain2',         Gains(17), ...
            'kneeStopGain',          Gains(18), ...
            'legAngleFilter',        Gains(19), ...
            'legLengthClr',          Gains(20), ...
            'simbiconGainD',         Gains(21), ...
            'simbiconGainV',         Gains(22), ...
            'simbiconLegAngle0',     Gains(23));

numSamples = 50;
terrains = 0:0.02:.14;
numTerrains = length(terrains);
totalRuns = numSamples*numTerrains;
dists = nan(totalRuns,1);

paramSets = cell(1,totalRuns);
idx = 1;
for i = 1:numTerrains
    for j = 1:numSamples
        [groundX, groundZ, groundTheta] = generateGround('const',terrains(i),idx);

        paramSets{idx} = Simulink.BlockDiagram.modifyTunableParameters(rtp, ...
            'groundZ',     groundZ, ...
            'groundTheta', groundTheta);
        idx = idx+1;
    end
end

parfor i = 1:length(paramSets)
    dists(i) = evaluateCostParallel(paramSets{i});
end
dists = reshape(dists,numSamples,[])
mediandists = nanmedian(dists)

save('distances.mat', 'dists')
